/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    FSM.h
  * @author  Mentos Seetoo
  * @brief   Finite State Machine base class.
  * @note    
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  * @attention
  * 
  * if you had modified this file, please make sure there is no any bug in your
  * code. Update the version Number and write dowm your name and the date.What 
  * most important is that make sure the users will have a clear understanding
  * through your description.
  ******************************************************************************
  */
#ifndef _FSM_H
#define _FSM_H
/* Includes ------------------------------------------------------------------*/
#include "Motion_Scheduler/GaitScheduler.h"
#include "Motion_Controller/BalanceController.h"
#include "Motion_Scheduler/DesiredCoMState.h"
#include "Stste_Feedback/StateEstimator.h"
#include "Dynamics/dynamicModel.h"
#include "sys_log.h"
#include <vector>
/* Types define --------------------------------------------------------------*/
namespace Quadruped
{
/**
 * @brief 
 */
enum class FSM_StateName
{
    PASSIVE,
    STAND_UP,
    LOCOMOTION,
    JUMP,
    BOUNCE
};

enum class FSM_OPStatus
{
    NORMAL,
    TRANSITIONING,
    STOP
};

/**
 * @brief Datas that state machine needs.
 */
template <typename T>
struct FSM_Data
{

};

/**
 * @brief Base class of Finite State Machine
 */
template <typename T>
class FSM_Base
{
protected:
    FSM_StateName _name;
    T _period;
    virtual void RunState() = 0;
    virtual int Transition(FSM_Base<T>* target_state) = 0;

    static FSM_Base<T>* GetStateHandle(FSM_StateName name);
    static FSM_Base<T>* current_state;
    static FSM_Base<T>* next_state;
    static FSM_OPStatus op_status;
    static bool SwitchState();
    /* data */
public:
    FSM_Base(T run_period){
        _period = run_period;
    }

    ~FSM_Base(){;};
    static void InitFSM(FSM_Base<T>* init_state);
    static void RunFSM();
    static void SetNextState(FSM_StateName name);
    static std::vector<FSM_Base<T>*> state_list; 

    FSM_StateName StateName(){ return _name ;};
};
} // namespace Quadruped
#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
